#ifndef _FRUSTRUM_CULLING
#define _FRUSTRUM_CULLING

#include <cmath>
#include <iostream>

class FrustrumCulling{
	public:
		Vector3 *fc, *ftl, *ftr, *fbl, *fbr, *nc, *ntl, *ntr, *nbl, *nbr, *up, *d, *right, *p,
						*nN, *fN, *tN, *bN, *rN, *lN;
		bool debug, enabled;
		Vector3 *normals[6];
		Vector3 *normalsP[6];
		FrustrumCulling(){
			debug = false;
			enabled = true;
			fc = new Vector3();
			ftl = new Vector3();
			ftr = new Vector3();
			fbl = new Vector3();
			fbr = new Vector3();
			nc = new Vector3();
			ntl = new Vector3();
			ntr = new Vector3();
			nbl = new Vector3();
			nbr = new Vector3();
		}
		void getFrustrumPlanePoints(float fov, float ratio, float lx,float ly,float lz,float ux,float uy,float uz,float near,float far){
			up = new Vector3(ux, uy, uz);
			d = new Vector3(lx, ly, lz);
			right = new Vector3();
			right->cross(*d, *up);
			p = new Vector3(0, 0, 0);
			fov = fov * M_PI / 180.0;
			float Hfar = 2.0 * tan(fov / 2.0) * far;
			float Wfar = Hfar * ratio;

			float Hnear = 2.0 * tan(fov / 2.0) * near;
			float Wnear = Hnear * ratio;


			*fc = *p + *d * far;

			*ftl = *fc + (*up * Hfar/2.0) - (*right * Wfar/2.0);
			*ftr = *fc + (*up * Hfar/2.0) + (*right * Wfar/2.0);
			*fbl = *fc - (*up * Hfar/2.0) - (*right * Wfar/2.0);
			*fbr = *fc - (*up * Hfar/2.0) + (*right * Wfar/2.0);

			*nc = *p + *d * near;
			*ntl = *nc + (*up * Hnear/2.0) - (*right * Wnear/2.0);
			*ntr = *nc + (*up * Hnear/2.0) + (*right * Wnear/2.0);
			*nbl = *nc - (*up * Hnear/2.0) - (*right * Wnear/2.0);
			*nbr = *nc - (*up * Hnear/2.0) + (*right * Wnear/2.0);

			if(debug){
				fc->printVector();
				nc->printVector();
				std::cout << "Hfar: " << Hfar << ", Wfar: " << Wfar << std::endl;	
				std::cout << "Hnear: " << Hnear << ", Wnear: " << Wnear << std::endl;	
				std::cout << "ftl: "; ftl->printVector();
				std::cout << "ftr: "; ftr->printVector();
				std::cout << "fbl: "; fbl->printVector();
				std::cout << "fbr: "; fbr->printVector();	
				std::cout << "ntl: "; ntl->printVector();
				std::cout << "ntr: "; ntr->printVector();
				std::cout << "nbl: "; nbl->printVector();
				std::cout << "nbr: "; nbr->printVector();
			}
			Vector3 *v = new Vector3(0, 0, -11);
	
			calculatePlaneNormals();
		}


		Vector3 *normal(Vector3 *p0, Vector3 *p1, Vector3 *p2){
			Vector3 *n = new Vector3();
			/*
			Compute vectors v = p1 – p0, and u = p2 – p0;
			Compute n = v x u (cross product)
			Normalize n
			*/
			p1->subtract(*p0);
			p2->subtract(*p0);
			n->cross(*p1, *p2);
			n->normalize();
			
			return n;
		}

		void calculatePlaneNormals(){
			nN = new Vector3();
			fN = new Vector3();
			tN = new Vector3();
			bN = new Vector3();
			rN = new Vector3();
			lN = new Vector3();
			Vector3 *normalV = new Vector3();
	
			// Near normal
			nN->set(*d);
				
			// Far normal
			fN->set(*d);
			fN->scale(-1);
				
			// Right normal
			normalV = normal(nbr, ntr, ftr);
			rN->set(*normalV);
				
			// Left normal
			normalV = normal(fbl, ftl, ntl);
			lN->set(*normalV);
				
			// Top normal (HARDCODE FOR PROGRESS)
			normalV = normal(ftr, ntr, ntl);
			tN->set(*normalV);
			tN->set(0.0, -0.87, -0.50);
	
			// Bottom normal
			normalV = normal(nbl, nbr, fbr);
			bN->set(*normalV);
			
			
			normals[0] = nN; normals[1] = fN; normals[2] = rN;
			normals[3] = lN; normals[4] = tN; normals[5] = bN;
			normalsP[0] = nc; normalsP[1] = fc; normalsP[2] = nbr;
			normalsP[3] = ftl; normalsP[4] = ftr; normalsP[5] = nbl;
		}

		int sphereInFrustum(Vector3 *p, float radius) {
			if(!enabled) return 2;
			
			float distance;
			int result = 2; // INSIDE
			for(int i=0; i < 6; i++){
				distance = p->dot(*normals[i]) - normals[i]->dot(*normalsP[i]);
	//			if(i == 3) std::cout << distance << std::endl;
				if (distance < -radius)
					return 0; // OUTSIDE
				else if (distance > -radius){
					result =  1; // INTERSECT
					//std::cout << "INTERSECT!\n";
					}
			}
			return(result);
		}		
	
};

#endif
